#include "utils.h"

//! Compares two double values with some tolerance
int CMP(double x, double y, double tol) {
	if(x > y+tol) return 1;
	else if(x+tol < y) return -1;
	else return 0;
}

//! Calculates the distance between two points in a 3D world
double squareDistance(Point3D p, Point3D q) {
	return ((p.x-q.x)*(p.x-q.x) + (p.y-q.y)*(p.y-q.y) + (p.z-q.z)*(p.z-q.z));
}

//! Calculates the distance between a segment and a point in a 3D world
double squareDistance(Point3D p, Segment3D s) {
	double t, res;
	vec3 direction = s.second-s.first, aux;
	aux = p-s.first;

	t = dot(direction, aux);
	t /= dot(direction, direction);

	Point3D qPrime;
	qPrime = s.first + t*direction;
	
	vec3 vet = p-qPrime;
	res = dot(vet, vet);
	if(CMP(t, 0.) < 0) {
		t = 0.0;
		vet = p-s.first;
		res = dot(vet, vet);
	}
	else if(CMP(t, 1.) > 0) {
		vet = p-s.second;
		res = dot(vet, vet);
	}

	return res;
}

//! Tests if the two segments intersect with robustness
bool segmentsIntersects(Segment3D l1, Segment3D l2) {
	vec3 d0, d1, u, res;
	double a, b, c, d, e, det, f, snum, sden, tnum, tden, s, t;
	d0 = l1.second-l1.first;
	d1 = l2.second-l2.first;
	u = l1.first-l2.first;

	a = dot(d0, d0);
	b = -(dot(d0, d1));
	c = dot(d1, d1);
	d = 2.*dot(u, d0);
	e = -2.*dot(u, d1);
	f = dot(u, u);
	
	det = 4.*(a*c - b*b);
	if(CMP(det, 0.0) == 0) {
		snum = 0.0;
		sden = 1.;
		tnum = -e;
		tden = 2.*c;
	}
	else {
		snum = 2.*(b*e - c*d);
		tnum = 2.*(b*d - a*e);
		sden = det;
		if(CMP(snum, 0.0) < 0) {
			snum = 0.;
			tnum = -e;
			tden = 2.*c;
		}
		else if(CMP(snum, det) > 0) {
			snum = det;
			tnum = -(e + 2.*b);
			tden = 2.*c;
		}
		else
			tden = det;
	}

	if(CMP(tnum, 0.0) < 0) {
		tnum = 0.;
		if(CMP(d, 0.0) > 0)
			snum = 0.0;
		else if(CMP(-d, 2.*a) > 0)
			snum = sden;
		else {
			snum = -d;
			sden = 2.*a;
		}
	}
	else {
		if(CMP(tnum, tden) > 0) {
			tnum = tden;
			if(CMP(2.*b + d, 0.0) > 0)
				snum = 0;
			else if(CMP(2.*b + d, -2.*a) < 0)
				snum = sden;
			else {
				snum = -(2.*b + d);
				sden = 2.*a;
			}
		}
	}

	s = snum/sden;
	t = tnum/tden;

	res = (l1.first + s*d0) - (l2.first + t*d1);

	return (CMP(dot(res, res), 0.0) == 0);
}

//! Tests if the given point intersect the given triangle in a 3D world
bool pointInTriangle(Point3D p0, Point3D p1, Point3D p2, Point3D p3) {
	vec3 e0, e1;
	Point3D q, paux;
	double s, t, a, b, c, d, e, f, det, invdet, dist;
	bool region0 = true;
	q = p1;
	e0 = p2-p1, e1 = p3-p1;
	a = dot(e0, e0);
	b = dot(e0, e1);
	c = dot(e1, e1);
	d = dot(e0, q-p0);
	e = dot(e1, q-p0);
	f = dot(q-p0, q-p0);

	det = a*c - b*b;
	s = b*e - c*d;
	t = b*d - a*e;
	if(CMP(s+t, det) <= 0) {
		if((CMP(s, 0.0) >= 0) && (CMP(t, 0.0) >= 0))
			region0 = true;
		else region0 = false;
	}
	else region0 = false;

	if(region0) {
		invdet = 1./det;
		s *= invdet;
		t *= invdet;
		paux = q + s*e0 + t*e1;
		return (CMP(squareDistance(paux, p0), 0.0) == 0);
	}
	else {
		dist = squareDistance(p0, make_pair(p1, p2));
		dist = min(dist, squareDistance(p0, make_pair(p1, p3)));
		dist = min(dist, squareDistance(p0, make_pair(p2, p3)));
		return (CMP(dist, 0.0) == 0);
	}
	return false;
}

//! Tests if the given segment intersect the given triangle in a 3D world
bool segtriIntersects(Point3D p0, Point3D p1, Point3D p2, Segment3D seg) {
	vec3 e1, e2, p, s, q;
	double t, u, v, tmp;
	e1 = p1-p0, e2 = p2-p0;
	p = cross(seg.second-seg.first, e2);
	tmp = dot(p, e1);

	if(CMP(tmp) < 0)
		return false;
	tmp = 1.0/tmp;
	s = seg.first - p0;
	u = tmp*dot(s, p);
	if(CMP(u, 0.0) < 0 || (CMP(u, 1.0) > 0))
		return false;

	q = cross(s, e1);
	v = tmp*dot(seg.second-seg.first, q);

	if(CMP(v, 0.0) < 0 || (CMP(v, 1.0) > 0))
		return false;

	if(CMP(u+v, 1.0) > 0)
		return false;

	t = tmp*dot(e2, q);

	return (CMP(t, 0.0) >= 0 || (CMP(t, 1.0) <= 0));
}

//! Tests if the given point intersect the given rectangle in a 3D world
bool pointInRectangle(Point3D p0, Point3D p1, Point3D p2, Point3D p3, Point3D p4) {
	//p1, p2, p3 e p4 are in counter-clockwise order
	vec3 e0, e1, d;
	Point3D p, q;
	p = p1;
	d = p0 - p;
	e0 = p2-p, e1 = p4-p;
	double s, t, dist, dot0, dot1;
	s = dot(e0, d);
	if(CMP(s, 0.0) > 0) {
		dot0 = dot(e0, e0);
		if(CMP(s, dot0) < 0) {
			d = d - (s / dot0)*e0;
		}
		else {
			d = d -e0;
		}
	}

	t = dot(e1, d);
	if(CMP(t, 0.0) > 0) {
		dot1 = dot(e1, e1);
		if(CMP(t, dot1) < 0) {
			d = d - (t / dot1)*e1;
		}
		else {
			d = d - e1;
		}
	}
	return (CMP(dot(d, d), 0.0) == 0);

}

//!	Tests if the given triangles intersects each other in a 3D world
bool triangleIntersects(Point3D p1, Point3D p2, Point3D p3, Point3D q1, Point3D q2, Point3D q3) {
	Segment3D a1, a2, a3, b1, b2, b3;
	a1 = make_pair(p1, p2);
	a2 = make_pair(p1, p3);
	a3 = make_pair(p2, p3);

	b1 = make_pair(q1, q2);
	b2 = make_pair(q1, q3);
	b3 = make_pair(q2, q3);
	return (segtriIntersects(q1, q2, q3, a1) || segtriIntersects(q1, q2, q3, a2) || segtriIntersects(q1, q2, q3, a3)
					|| segtriIntersects(p1, p2, p3, b1) || segtriIntersects(p1, p2, p3, b2) || segtriIntersects(p1, p2, p3, b3));
}

//! Tests if the given rectangles intersects each other in a 3D world
bool rectangleIntersects(Point3D p1, Point3D p2, Point3D p3, Point3D p4, Point3D q1, Point3D q2, Point3D q3, Point3D q4) {
	return (triangleIntersects(p1, p2, p3, q1, q2, q3) || triangleIntersects(p1, p2, p3, q1, q3, q4) 
					|| triangleIntersects(p1, p3, p4, q1, q2, q3) || triangleIntersects(p1, p3, p4, q1, q3, q4));
}

//! Tests if the triangle and the rectangle intersects each other in a 3D world
bool triangleRectangleIntersects(Point3D p1, Point3D p2, Point3D p3, Point3D q1, Point3D q2, Point3D q3, Point3D q4) {
	return (triangleIntersects(p1, p2, p3, q1, q2, q3) || triangleIntersects(p1, p2, p3, q1, q3, q4));
}

